#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"

double getDerivative(double errors[],int length);

int main(void){
	LockoutProtection();
	InitializeMCU();
	InitializeMotors(false, false);
	InitializeLineSensor();

	//This boolean is set to true if the background is black and line is white. False if the opposite
	tBoolean lineIsWhite=False;

	//PID values
	double Kp=1;
	double Ki=0;
	double Kd=0;
	double error=0,lastError=0;
	double integral=0;
	double derivative=0;
	//Line Sensor values
	unsigned char lineSensorByte = ReadLineSensor();
 	int lineSensorArray[8];
	int i;
	//Motor Values
	signed char maxSpeed=127;

	while(1){
		lastError=error;
		//Get line sensor values; 1 means black
		lineSensorByte = ReadLineSensor();
		if(lineIsWhite)lineSensorByte=~lineSensorByte;//Used to change between white and black lines
		for(i = 0; i < 8; i++) {
			lineSensorArray[i] = lineSensorByte & 0x01;
			lineSensorByte = lineSensorByte >> 1;
		}
		//Now it's time to compute the error... the algorithm is as follows:
		//Each bit has a value. Starting from the MSB and going right, the values are:
		//7, 5, 3, 1, -1, -3, -5, -7
		//These values are summed and averaged to get the error
		//NOTE: THIS IS FOR A BLACK LINE!!! FOR A WHITE LINE, INVERT THE VALUES
		error=0;
		int lineCount=0;
		for(int i=0;i<8;i++){
			error+=lineSensorArray[i]*(7-(i<<1));
			lineCount+=lineSensorArray[i];
		}
		if(lineCount!=0)error/=lineCount;
		else{
			error=8;
			if(lastError<0)error*=-1;
		}
		//Now that we have the error, we can update the derivative and integral
		derivative=error-lastError;
		integral+=error;
		//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
		//It should be noted that a positive derivative means the robot needs to turn left, and a
		//negative derivative means the robot needs to turn right. A positive integral means the
		//robot needs to skew left, and a negative integral value means the robot needs to skew right
		double Pout=Kp*error;
		double Iout=Ki*integral;
		double Dout=Kd*derivative;
		double MV=Pout+Iout+Dout;
		//If MV is positive, the robot needs to go left. If MV is negative, the robot needs to go right
		//But... we first need to come up with a linear relationship relating MV to the speed of the
		//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
		double absMV=MV;
		if(MV<0)absMV*=-1;
		double m=10;//This is the value that you change for sharper turning!!!
		signed char velocity=(signed char)(maxSpeed-m*absMV);
		if(MV>0){
			SetMotorPowers(maxSpeed,velocity);
		}
		else if(MV<0){
			SetMotorPowers(velocity,maxSpeed);
		}
		else{//MV equals 0; never going to happen, but might?
			SetMotorPowers(maxSpeed,maxSpeed);
		}
	}
}